// -*- Mode: Java -*-
// Plan to fly to a specific waypoint

//Commands
Boolean Command CheckDirectPathFeasibility(Real fromPosition[3],Real toPosition[3]);
Integer Command FindNewPath(String algorithm,Real fromPosition[3],Real fromVelocity[3],Real toPosition[3]);
Real[3] Command GetResolutionWP(Integer index);
Real Command ComputeDistance(Real fromPosition[3],Real toPosition[3]);
Command SetPos(Real position[3]);
Command SetMode(String mode);
Command SetVel(Real velocity[3]);
Command SetSpeed(Real speed);
Command pprint(...);

// Lookups
Real[3] Lookup position;
Real[3] Lookup velocity;
Real[3] Lookup nextFeasibleWP;
Boolean Lookup directPathToNextFeasibleWP;
Boolean Lookup restartMission;
Integer Lookup totalResolutionWP;

LibraryAction MONITOR(InOut Integer ConflictId);

GOTO_POSITION:{

    In String SearchType;
    In Boolean TrafficConflict;
    In Real TargetPosition[3];
    In Real resSpeed;
    In Integer PrevConflictId;
    InOut Boolean resolutionCompleted;
    Real CurrentPosition[3];
    Real CurrentVelocity[3];
    Real capH;
    Integer nextWPIndex = 1;
    Integer totalWPIndex;
    Boolean targetFeasibility;

    EndCondition isKnown(resolutionCompleted);

    pprint("Plexil status: In Library call GOTO_POSITION");

    CurrentPosition = Lookup(position);
    CurrentVelocity = Lookup(velocity);
    capH = resSpeed;

    SetMode("ACTIVE");
    
    GET_FEASIBLE_WP:{

        pprint("Plexil status: Checking target feasibility");

        {
            EndCondition isKnown(targetFeasibility);
            targetFeasibility = CheckDirectPathFeasibility(CurrentPosition,TargetPosition);
        }

        pprint("Plexil status: Target feasibility ",targetFeasibility);

	    {
          	SkipCondition !TrafficConflict;
           	targetFeasibility = false;
        }

    }


    BEE_LINE:{
        Integer vConflictId;
        Real dist2nextWP = 10000;
        Boolean reset = false;
        SkipCondition !targetFeasibility;

        pprint("Plexil status: Direct to target position");
        vConflictId = PrevConflictId;

        SetPos(TargetPosition);
        SetSpeed(resSpeed);

        {
            Real val;
            Boolean reset = false;
            Repeat (dist2nextWP > capH) && (vConflictId == PrevConflictId) && (!reset);
            CurrentPosition = Lookup(position);
            reset = Lookup(restartMission);

            {
                EndCondition isKnown(val);
                val = ComputeDistance(CurrentPosition,TargetPosition);
            }
            dist2nextWP = val;

            LibraryCall MONITOR(ConflictId = vConflictId);
        }

        resolutionCompleted = true;
    }

    PLAN_PATH:{
        Integer status;
        SkipCondition targetFeasibility;

        pprint("Plexil status: Planning a path to target location");

        {
            EndCondition isKnown(status);
            status = FindNewPath(SearchType,CurrentPosition,CurrentVelocity,TargetPosition);
        }

        if(status < 0){
            resolutionCompleted = false;
            pprint("Plexil status: Failed to Compute resolution");
        }endif

        totalWPIndex = Lookup(totalResolutionWP);

        pprint("Plexil status: Computed waypoint with:",SearchType);

    }


    CHECK_WP_TRANSITION:{
        Real NextWP[3];
        Integer vConflictId;
        Boolean reset = false;

        vConflictId = PrevConflictId;

        {
            EndCondition isKnown(NextWP[0]);
            NextWP = GetResolutionWP(nextWPIndex);
        }

        // Use set pos to go to spot directly
        SetPos(NextWP);
        reset = Lookup(restartMission);

        {
            Real dist2nextWP;
            Real heading2nextWP;
            Real velocityCmd[3];
            Repeat (nextWPIndex < totalWPIndex) && (vConflictId == PrevConflictId) && (!reset);

            CurrentPosition = Lookup(position);
            reset = Lookup(restartMission);

            LibraryCall MONITOR(ConflictId = vConflictId);

            {
                EndCondition isKnown(dist2nextWP);
                dist2nextWP = ComputeDistance(CurrentPosition,NextWP);
            }

            {
                SkipCondition dist2nextWP > capH;
                nextWPIndex = nextWPIndex + 1;

                {
                    Real _nextWP[3];
                    SkipCondition (nextWPIndex >= totalWPIndex);

                    {
                        EndCondition isKnown(_nextWP[0]);
                        _nextWP = GetResolutionWP(nextWPIndex);
                    }

                    SetPos(_nextWP);
                    NextWP[0] = _nextWP[0];
                    NextWP[1] = _nextWP[1];
                    NextWP[2] = _nextWP[2];
                }

            }

        }

        resolutionCompleted = true;
    }
}
